/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.impulserobotics;

/**
 *
 * @author Sahil Jain
 */
import com.sun.squawk.util.MathUtils;
import edu.wpi.first.wpilibj.networktables.NetworkTableKeyNotDefined;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class PhysicsCalculator {
    
        //Physics constants
    public static final double gravityMagnitude = 9.8;
    double rHeight;
    double thetaRads;
    double tanTheta;
    double cos2Theta;
    
    
    public PhysicsCalculator(){
        rHeight = Constants.SHOOTER_HEIGHT;
        thetaRads = Math.toRadians(Constants.SHOOTER_ANGLE);
        tanTheta = Math.tan(thetaRads);
        cos2Theta = MathUtils.pow(Math.cos(thetaRads), 2);
    }
    
    /**
     * Returns the magnitude of the speed the jaguars should output in m/s.
     * @return a double value of the speed in m/s (not pwm output!)
     * @param dx the distance from the tip of the shooter to the middle of the net.
     * @param netHeight the height of the rim of the net from the ground.
     */
    public double getSpeed(double camServoVerticalAngle) {
        
       
        
        double netHeight = Constants.MID_NET_HEIGHT;
        try {
            switch((int)SmartDashboard.getInt("netLevel")){
                case 0:
                    netHeight = Constants.LOW_NET_HEIGHT;
                    break;
                case 1:
                    netHeight = Constants.MID_NET_HEIGHT;
                    break;
                case 2:
                    netHeight = Constants.HIGH_NET_HEIGHT;
                    break;
                 
            }
        } catch (NetworkTableKeyNotDefined ex) {
            ex.printStackTrace();
        }
         double camAngleRads = Math.toRadians(camServoVerticalAngle);
        double dx = (netHeight-Constants.CAM_HEIGHT)/Math.tan(camAngleRads)-Constants.CAMERA_SHOOTER_DIFFERENCE;
        
        
        double dy = netHeight- rHeight;
        double numerator = gravityMagnitude/2 *MathUtils.pow(dx, 2);
       // double denominator = Math.tan(thetaRads)*dx*MathUtils.pow(Math.cos(thetaRads), 2)-;
        double denominator = cos2Theta*(tanTheta*dx - dy);
        double output = numerator/denominator;
        return output;
                
    }
}
